#include <stdio.h>
#include <stdlib.h>
#include <csp_spu.h>
#include <csp_debug.h>
#include <malloc_align.h>
#include <liblarge_matrix.h>
#include <spu_intrinsics.h>
#include <spu_mfcio.h>
#include <libsync.h>
#include <alloca.h>
#include "../../ppc/sor_benchmark2/sor.h"





int global_pid; 

init_data_t init __attribute__ ((aligned(128)));

csp_channel init_channel;
csp_channel top_in, top_out;
csp_channel bottom_in, bottom_out;

float *data, *data_end, *tGhostL, *bGhostL;



#define TAG 25


void setup(float *trap_data, int height, int width){
	int x,y;
	/* Initial state */
	for(y=0;y<height-1;y++)           //Setup left side temp
		trap_data[y*width]=-273.15;

	for(y=0;y<height-1;y++)           //Setup right side temp
		trap_data[y*width+(width-1)]=-273.15;

	for(x=0; x<width; x++)            //Setup top temp
		trap_data[0*width+x]=40.0;

	for(x=0; x<width; x++)            //Setup bottom temp
		trap_data[(height-1)*width+x]=-273.15;
}







void read_data(float *to_, ea_addr_t from, size_t size_){
	int i = 0;
	unsigned int sz;
	int size = size_;
	void *to = to_;
	while(size > 0){
		sz = (size < _16KB) ? size : _16KB;
		size -= sz;
		i += sz;
		//		printf("mfc_get(%p, %p, %i, %i, 0, 0) stribe=%i, size=%i\n", 
		//				to, (void*)from, sz, TAG, init.stribe, size_);
		mfc_get(to, from, sz, TAG, 0, 0); 
		mfc_write_tag_mask(1<<TAG);
		mfc_read_tag_status_all();
		to += sz;
		from += sz;
	}
	//	printf("%i bytes read\n", i);
}

void write_data(float *from_, ea_addr_t to_, size_t size_){
	int i = 0;
	unsigned int sz;
	ea_addr_t to = to_;
	void *from = from_;
	size_t size = size_;
	while(size > 0){
		sz = (size < _16KB) ? size : _16KB;
		size -= sz;
		i += sz;
		mfc_put(from, to, sz, TAG, 0, 0); 
		mfc_write_tag_mask(1<<TAG);
		mfc_read_tag_status_all();
		to += sz;
		from += sz;
	}

	//	printf("%i bytes written\n", i);
}

void calcVectorUnrollHeapAlloc(int rows, int cols, float *dFrom) // {{{ --
{
	//char *data[rows*cols*sizeof(float)*2+128];
	//char *data[HEIGHT*WIDTH*sizeof(float)*2+128];
	void *data = alloca(rows*cols*sizeof(float)*2+128);

	float *dT1 = (float*) mfc_ceil128( (unsigned int) data);
	float *dT2 = &dT1[rows*cols];


	vector float* vFrom	 = (vector float*) dFrom;
	//vector float* vFromT = (vector float*) dFromT;
	//vector float* vTo    = (vector float*) dTo;
	vector float* vT1    = (vector float*) dT1;
	vector float* vT2    = (vector float*) dT2;
	//vector float* vT3    = (vector float*) dT3;

	vector float v0, v1, v2, v3, v4, v5, v6, v7;
	vector float v01, v11, v21, v31, v41, v51, v61, v71;

	int r,c;

	/* Transponer vFrom -> vFromT */
	transpose_matrix(
			rows, 
			cols, 
			dFrom, cols, 
			dT2,rows);

	//print(dFromT, WIDTH,  HEIGHT,"dFromT");

	int vTCols = rows/4;
	int vTRows = cols;
	for (r = 0; r < vTRows; r++){
		for (c = 1; c < vTCols-1; c+=8){ // {{{ --

			int r0c0_index = r*vTCols + c;
			int r0c1_index = r0c0_index +1;
			int r0c2_index = r0c0_index +2;
			int r0c3_index = r0c0_index +3;
			int r0c4_index = r0c0_index +4;
			int r0c5_index = r0c0_index +5;
			int r0c6_index = r0c0_index +6;
			int r0c7_index = r0c0_index +7;

			int rm1c0_index = r0c0_index - vTCols;
			int rm1c1_index = r0c1_index - vTCols;
			int rm1c2_index = r0c2_index - vTCols;
			int rm1c3_index = r0c3_index - vTCols;
			int rm1c4_index = r0c4_index - vTCols;
			int rm1c5_index = r0c5_index - vTCols;
			int rm1c6_index = r0c6_index - vTCols;
			int rm1c7_index = r0c7_index - vTCols;

			int rp1c0_index = r0c0_index + vTCols;
			int rp1c1_index = r0c1_index + vTCols;
			int rp1c2_index = r0c2_index + vTCols;
			int rp1c3_index = r0c3_index + vTCols;
			int rp1c4_index = r0c4_index + vTCols;
			int rp1c5_index = r0c5_index + vTCols;
			int rp1c6_index = r0c6_index + vTCols;
			int rp1c7_index = r0c7_index + vTCols;

			int tc0_index = r* (vTCols)+c;
			int tc1_index = tc0_index+1;
			int tc2_index = tc0_index+2;
			int tc3_index = tc0_index+3;
			int tc4_index = tc0_index+4;
			int tc5_index = tc0_index+5;
			int tc6_index = tc0_index+6;
			int tc7_index = tc0_index+7;

			v0 = spu_add(vT2[rm1c0_index], vT2[rp1c0_index]);
			v1 = spu_add(vT2[rm1c1_index], vT2[rp1c1_index]);
			v2 = spu_add(vT2[rm1c2_index], vT2[rp1c2_index]);
			v3 = spu_add(vT2[rm1c3_index], vT2[rp1c3_index]);
			v4 = spu_add(vT2[rm1c4_index], vT2[rp1c4_index]);
			v5 = spu_add(vT2[rm1c5_index], vT2[rp1c5_index]);
			v6 = spu_add(vT2[rm1c6_index], vT2[rp1c6_index]);
			v7 = spu_add(vT2[rm1c7_index], vT2[rp1c7_index]);

			vT1[tc0_index] = v0;
			vT1[tc1_index] = v1;
			vT1[tc2_index] = v2;
			vT1[tc3_index] = v3;
			vT1[tc4_index] = v4;
			vT1[tc5_index] = v5;
			vT1[tc6_index] = v6;
			vT1[tc7_index] = v7;
		} // }}} --
	}

	//print(dT2,  WIDTH, HEIGHT,"dT2 efter addition");

	/* Transponer vT2 -> vT3 */
	transpose_matrix(
			cols, 
			rows, 
			dT1,rows, 
			dT2,cols);

	//print(dT3, HEIGHT, WIDTH, "dT3 transponeret");



	//print(dFrom, HEIGHT, WIDTH, "dFrom");
	//print(dT1, HEIGHT, WIDTH, "dT1 before");

	int vCols = cols/4;
	int vRows = rows;
	for (r = 1; r < vRows-1; r++){
		for (c = 0; c < vCols; c+=8) // {{{ ---
		{
			int r0c0_index = r*vCols + c;
			int r0c1_index = r0c0_index +1;
			int r0c2_index = r0c0_index +2;
			int r0c3_index = r0c0_index +3;
			int r0c4_index = r0c0_index +4;
			int r0c5_index = r0c0_index +5;
			int r0c6_index = r0c0_index +6;
			int r0c7_index = r0c0_index +7;

			int rm1c0_index = r0c0_index - vCols;
			int rm1c1_index = r0c1_index - vCols;
			int rm1c2_index = r0c2_index - vCols;
			int rm1c3_index = r0c3_index - vCols;
			int rm1c4_index = r0c4_index - vCols;
			int rm1c5_index = r0c5_index - vCols;
			int rm1c6_index = r0c6_index - vCols;
			int rm1c7_index = r0c7_index - vCols;

			int rp1c0_index = r0c0_index + vCols;
			int rp1c1_index = r0c1_index + vCols;
			int rp1c2_index = r0c2_index + vCols;
			int rp1c3_index = r0c3_index + vCols;
			int rp1c4_index = r0c4_index + vCols;
			int rp1c5_index = r0c5_index + vCols;
			int rp1c6_index = r0c6_index + vCols;
			int rp1c7_index = r0c7_index + vCols;
			/*
			   int tc0_index = r*(vCols)+c;
			   int tc1_index = tc0_index+1;
			   int tc2_index = tc0_index+2;
			   int tc3_index = tc0_index+3;
			   int tc4_index = tc0_index+4;
			   int tc5_index = tc0_index+5;
			   int tc6_index = tc0_index+6;
			   int tc7_index = tc0_index+7;
			   */
			v0 = spu_add(vFrom[r0c0_index], vFrom[rm1c0_index]);
			v1 = spu_add(vFrom[r0c1_index], vFrom[rm1c1_index]);
			v2 = spu_add(vFrom[r0c2_index], vFrom[rm1c2_index]);
			v3 = spu_add(vFrom[r0c3_index], vFrom[rm1c3_index]);
			v4 = spu_add(vFrom[r0c4_index], vFrom[rm1c4_index]);
			v5 = spu_add(vFrom[r0c5_index], vFrom[rm1c5_index]);
			v6 = spu_add(vFrom[r0c6_index], vFrom[rm1c6_index]);
			v7 = spu_add(vFrom[r0c7_index], vFrom[rm1c7_index]);

			v01 = spu_add(vFrom[rp1c0_index], v0);
			v11 = spu_add(vFrom[rp1c1_index], v1);
			v21 = spu_add(vFrom[rp1c2_index], v2);
			v31 = spu_add(vFrom[rp1c3_index], v3);
			v41 = spu_add(vFrom[rp1c4_index], v4);
			v51 = spu_add(vFrom[rp1c5_index], v5);
			v61 = spu_add(vFrom[rp1c6_index], v6);
			v71 = spu_add(vFrom[rp1c7_index], v7);
			/*
			   vT1[tc0_index] = v01;
			   vT1[tc1_index] = v11;
			   vT1[tc2_index] = v21;
			   vT1[tc3_index] = v31;
			   vT1[tc4_index] = v41;
			   vT1[tc5_index] = v51;
			   vT1[tc6_index] = v61;
			   vT1[tc7_index] = v71;
			   */

			vT1[r0c0_index] = v01;
			vT1[r0c1_index] = v11;
			vT1[r0c2_index] = v21;
			vT1[r0c3_index] = v31;
			vT1[r0c4_index] = v41;
			vT1[r0c5_index] = v51;
			vT1[r0c6_index] = v61;
			vT1[r0c7_index] = v71;




		} // }}} ---
	}
	//print(dT3, HEIGHT, WIDTH, "vT2");




	//print(dT1, HEIGHT, WIDTH, "vT1");
	//print(dT2, HEIGHT, WIDTH, "vT2");

	int vSCols = cols/4;
	int vSRows = rows;
	vector float w = spu_splats((float)0.2);
	for (r = 1; r < vSRows-1; r++){
		float left  = dFrom[r*cols];
		float right = dFrom[(r+1)*cols-1];
		for (c = 0; c < vSCols-0; c += 8) // {{{ --
		{
			int r0c0_index = (r)*vSCols + c;
			int r0c1_index = r0c0_index +1;
			int r0c2_index = r0c0_index +2;
			int r0c3_index = r0c0_index +3;
			int r0c4_index = r0c0_index +4;
			int r0c5_index = r0c0_index +5;
			int r0c6_index = r0c0_index +6;
			int r0c7_index = r0c0_index +7;


			v0 =spu_add(vT1[r0c0_index], vT2[r0c0_index]);
			v1 =spu_add(vT1[r0c1_index], vT2[r0c1_index]);
			v2 =spu_add(vT1[r0c2_index], vT2[r0c2_index]);
			v3 =spu_add(vT1[r0c3_index], vT2[r0c3_index]);
			v4 =spu_add(vT1[r0c4_index], vT2[r0c4_index]);
			v5 =spu_add(vT1[r0c5_index], vT2[r0c5_index]);
			v6 =spu_add(vT1[r0c6_index], vT2[r0c6_index]);
			v7 =spu_add(vT1[r0c7_index], vT2[r0c7_index]);

			v01 = spu_mul(v0,w);
			v11 = spu_mul(v1,w);
			v21 = spu_mul(v2,w);
			v31 = spu_mul(v3,w);
			v41 = spu_mul(v4,w);
			v51 = spu_mul(v5,w);
			v61 = spu_mul(v6,w);
			v71 = spu_mul(v7,w);

			vFrom[r0c0_index] = v01;
			vFrom[r0c1_index] = v11;
			vFrom[r0c2_index] = v21;
			vFrom[r0c3_index] = v31;
			vFrom[r0c4_index] = v41;
			vFrom[r0c5_index] = v51;
			vFrom[r0c6_index] = v61;
			vFrom[r0c7_index] = v71;
		} // }}} --
		dFrom[r*cols] = left;
		dFrom[(r+1)*cols-1] = right;
	}
	//print(dFrom, HEIGHT, WIDTH, "vT1+VT3 = vFrom");

} // }}} --


void CALCBODY(){
	//int x,y;
	//for(y=1; y < init.height-1; y++){
	//	for(x=1; x < init.width-1; x++){
	//		data[y*init.width+x] = 0.2 * (
	//				data[y*init.width+x] + // current
	//				data[(y-1)*init.width+x] + // top
	//				data[(y+1)*init.width+x] + // bottom
	//				data[y*init.width+(x-1)] + // left
	//				data[y*init.width+(x+1)]); // right
	//	}
	//}

	calcVectorUnrollHeapAlloc(init.height, init.width, data);
}

//void CALCBODY2(){
//	int x,y;
//	//for(y=1; y < init.height-1; y++){
//	//	for(x=1; x < init.width-1; x++){
//	//		data[y*init.width+x] = 0.2 * (
//	//				data[y*init.width+x] + // current
//	//				data[(y-1)*init.width+x] + // top
//	//				data[(y+1)*init.width+x] + // bottom
//	//				data[y*init.width+(x-1)] + // left
//	//				data[y*init.width+(x+1)]); // right
//	//	}
//	//}
//	//calcVectorUnrollHeapAlloc(init.height, init.width, data);
//}


void CALCTOP(){
	int x;
	for(x=1; x < init.width-1; x++){
		data[x] = 0.2 * (
				data[x] + // current
				tGhostL[x] + // top
				data[init.width+x] + // bottom
				data[x-1] + // left
				data[x+1]); // right
	}
}

void CALCBOTTOM(){
	int x;
	int y = (init.height-1)*init.width;
	for(x=1; x < init.width-1; x++){
		data[y+x] = 0.2 * (
				data_end[x] + // current
				data[(y-init.width)+x] + // top
				bGhostL[x] + // bottom
				data[y+(x-1)] + // left
				data[y+(x+1)]); // right
	}
}
//
//void red_solver(){
//	int iterations = ITERATIONS;
//
//	printf("slow down\n");
//	SLOW_DOWN();
//
//	if( 0 != init.cBottomOut ){
//		CALCBOTTOM();
//		csp_channel_write(bottom_out, data_end);
//		//printMatrix(data_end, 1, WIDTH, "skriver data_end", csp_pid());
//	}
//	if( 0 != init.cTopOut ){
//		//printMatrix(data, 1, WIDTH, "skriver data_top", csp_pid());
//		CALCTOP();
//		csp_channel_write(top_out, data);	
//	}
//
//	while( iterations-- ){
//
//		SLOW_DOWN();
//
//		if( 0 != init.cBottomIn ){
//			csp_channel_read(bottom_in, bGhostL);
//			printf("red   bottom\n");
//			CALCBOTTOM();
//			csp_channel_write(bottom_out, data_end);
//		}
//
//		//printMatrix(data, HEIGHT, WIDTH, "red", csp_pid());
//		//SLOW_DOWN();
//		printf("red   body\n");
//		CALCBODY();
//		if( 0 != init.cTopIn ){
//			csp_channel_read(top_in, tGhostL);	
//			printf("red   top\n");
//			CALCTOP();
//			csp_channel_write(top_out, data);	
//		}
//
//
//	}
//}
//
//void black_solver(){
//	int iterations = ITERATIONS;
//	while( iterations-- ){
//
//		if( 0 != init.cTopIn ){
//			csp_channel_read(top_in, tGhostL);
//			printf("black top\n");
//			CALCTOP();
//			csp_channel_write(top_out, data);
//		}
//
//		//printMatrix(data, HEIGHT, WIDTH, "black", csp_pid());
//		//SLOW_DOWN();
//		printf("black body\n");
//		CALCBODY();
//
//
//		if( 0 != init.cBottomIn ){
//			csp_channel_read(bottom_in, bGhostL);	
//			printf("black bottom\n");
//			CALCBOTTOM();
//			csp_channel_write(bottom_out, data_end);	
//		}
//	}
//	if( 0 != init.cBottomIn ){
//		csp_channel_read(bottom_in, bGhostL);	
//	}
//	if( 0 != init.cTopIn ){
//		csp_channel_read(top_in, tGhostL);
//	}
//}

//init_data_t do_init(){
//	int i;
//	init_data_t init[N_WORKERS];
//	init_data_t sorted_init[N_WORKERS];
//
//	init[0].cTopIn     = 0; 
//	init[0].cTopOut    = 0;
//	init[0].stribe     = 0;
//	init[0].color      = 0;
//	init[0].cBottomIn  = CBASE_IN;
//	init[0].cBottomOut = CBASE_OUT; 
//	init[0].width	   = WIDTH;
//	init[0].height 	   = HEIGHT / N_WORKERS;
//
//	//csp_channel_write(init_o2a, &init[0]);
//
//	for(i=1; i < N_WORKERS; i++){
//		init[i].cTopIn     = init[i-1].cBottomOut;  
//		init[i].cTopOut    = init[i-1].cBottomIn;
//		init[i].cBottomIn  = CBASE_IN  + i + 1;
//		init[i].cBottomOut = CBASE_OUT + i + 1;
//		init[i].stribe = i;
//		if( N_WORKERS-1 == i ){
//			init[i].cBottomIn=0;
//			init[i].cBottomOut=0; 
//		}
//		init[i].color     = i % 2;
//		init[i].width  = WIDTH;
//		init[i].height = HEIGHT / N_WORKERS;
//
//
//		//csp_channel_write(init_o2a, &init[i]);
//
//		//		init[i].cTopIn     = init.cBottomOut;  
//		//		init[i].cTopOut    = init.cBottomIn;
//		//		init[i].cBottomIn  = CBASE_IN  + i + 1;
//		//		init[i].cBottomOut = CBASE_OUT + i + 1;
//	}
//
//	int t=0;
//	for(i=0; i < N_WORKERS; i+=2){sorted_init[t] = init[i];t++;}
//	for(i=1; i < N_WORKERS; i+=2){sorted_init[t] = init[i];t++;}
//
//
//
//	int idx = csp_pid() % N_WORKERS;
//	printf("idx=%i chosen\n", idx);
//	return sorted_init[idx];
//}


int Run(){
	int pid = csp_pid();

	global_pid = csp_pid();

	init_channel = csp_create_channel(
			ONE2ANY_CHANNEL, INIT_CHANNEL, sizeof(init_data_t));

	csp_channel_read(init_channel, &init);
	//init = do_init();

	//printf("sor WORKER code_size=%i, "
	//		"cTopIn=%llx, cTopOut=%llx, cBotIn=%llx, cBotOut%llx, color=%s, pid=%i \n", 
	//		csp_code_size(),
	//		init.cTopIn,    
	//		init.cTopOut,   
	//		init.cBottomIn,  
	//		init.cBottomOut,
	//		RED == init.color ? "RED": "BLACK",
	//		pid);

	top_in     = csp_create_channel(ONE2ONE_CHANNEL, init.cTopIn    , init.width*sizeof(float));
	top_out    = csp_create_channel(ONE2ONE_CHANNEL, init.cTopOut   , init.width*sizeof(float));
	bottom_in  = csp_create_channel(ONE2ONE_CHANNEL, init.cBottomIn , init.width*sizeof(float));
	bottom_out = csp_create_channel(ONE2ONE_CHANNEL, init.cBottomOut, init.width*sizeof(float));

	//printf("WORKER shared address: %x\n", csp_shared_addr());

	int data_size = init.width*init.height;
	int buff_size = init.width*(init.height + 2); // incl. ghostlines
	tGhostL  = (float*) _malloc_align(buff_size*sizeof(float), 7);
	data     = &tGhostL[init.width];
	data_end = &tGhostL[buff_size - (2*init.width)];
	bGhostL  = &tGhostL[buff_size - init.width];

	ea_addr_t data_ea = csp_shared_addr() + (sizeof(float)*data_size*init.stribe);


	//printf("#%i: iterations=%i, codesize=%i, datasize=%i, bufsize=%i, data_ea=%p, "
	//		"width=%i, height=%i, "
	//		"tGghostL=%p, data=%p, data_end=%p, bGhostL=%p\n",
	//		ITERATIONS, init.stribe, csp_code_size(), data_size, buff_size, (void*) data_ea,
	//		init.width, init.height,
	//		tGhostL, data, data_end, bGhostL);

	read_data(data, data_ea, sizeof(float)*data_size);

	//printf("sor_worker2\n");

	int it = ITERATIONS;
	if( 0 == init.stribe%2 ){
		while(it--){
			// S
			if( __builtin_expect((0 < init.cBottomOut), 1)){ 
				//CALCBOTTOM();
				csp_channel_write(bottom_out, data_end);
			}
			if( __builtin_expect((0 < init.cTopOut), 1)){ 
				//CALCTOP();
				csp_channel_write(top_out, data);
			}
			CALCBODY();
			if( __builtin_expect((0 < init.cBottomIn), 1)){  
				csp_channel_read(bottom_in, bGhostL);
				CALCBOTTOM();
			}
			if( __builtin_expect((0 < init.cTopIn), 1)){  
				csp_channel_read(top_in, tGhostL);
				CALCTOP();
			}
			// R
			//if( 0 != init.cBottomOut ){ csp_channel_write( bottom_out, tGhostL );}
			//if( 0 != init.cTopOut ){    csp_channel_write( top_out,    tGhostL );}
			//if( 0 != init.cBottomIn ){  csp_channel_read(  bottom_in,  bGhostL );}
			//if( 0 != init.cTopIn ){     csp_channel_read(  top_in,     bGhostL );}
		}
	}else{
		while(it--){
			// S
			if( __builtin_expect((0 < init.cTopIn), 1) ){ 
				csp_channel_read(top_in, tGhostL);
				CALCTOP();
			}
			CALCBODY();
			if( __builtin_expect((0 < init.cBottomIn ), 1) ){ 
				csp_channel_read(bottom_in,  bGhostL);
				CALCBOTTOM();
			}
			if( __builtin_expect((0 < init.cTopOut), 1) ){ 
				csp_channel_write(top_out, data);
			}
			if( __builtin_expect((0 < init.cBottomOut), 1) ){ 
				csp_channel_write(bottom_out, data_end);

			}
			// R
			//if( 0 != init.cTopIn ){     csp_channel_read(  top_in,     bGhostL );}
			//if( 0 != init.cBottomIn ){  csp_channel_read(  bottom_in,  bGhostL );}
			//if( 0 != init.cTopOut ){    csp_channel_write( top_out,    tGhostL );}
			//if( 0 != init.cBottomOut ){ csp_channel_write( bottom_out, tGhostL );}
		}
	}


	write_data(data, data_ea, sizeof(float)*data_size);

	ASSERT( pid == csp_pid() );
	ASSERT( global_pid == csp_pid() );


	//printf("sor_worker exit\n");

	return 0;
}
